#include "camera.h"

Camera::Camera()
{
}

int Camera::connectCam(int index)
{
    capture = cvCaptureFromCAM(index);
    if( !capture )
    {
        alert.setText("Camera not connected");
        alert.exec();
        return 0;
    }

    //cvNamedWindow("Image", CV_WINDOW_AUTOSIZE);

    return 1;
}

IplImage* Camera::openImage(QString fileName)
{
    QByteArray fileNameByte;
    char* fileNameChar;

    fileNameByte = fileName.toLatin1();
    fileNameChar = fileNameByte.data();
    return (cvLoadImage(fileNameChar));
}

IplImage* Camera::getImage()
{
    cvQueryFrame(capture);
    return cvQueryFrame(capture);
}

IplImage* Camera::getImageSave(int imgNum)
{
    char *fname;

    fname = (char*) malloc(sizeof(char) * 30);

    fname = itoa(imgNum, fname, 10);
    fname = strcat(fname, ".jpg");

    imgColor = cvQueryFrame(capture);
    imgColor = cvQueryFrame(capture);
    imgColor = cvQueryFrame(capture);
    imgColor = cvQueryFrame(capture);

    cvSaveImage(fname, imgColor);

    return imgColor;
}

void Camera::showImage()
{
    cvShowImage("Image", getImage());
}

bool Camera::insideCircle()
{
    int high, low;
    imgColor = getImageSave(2);
    IplImage *img = cvCreateImage(cvGetSize(imgColor), 8, 1);
    cvCvtColor(imgColor, img, CV_BGR2GRAY);
    cvThreshold(img, img, 128, 255, CV_THRESH_BINARY);

    high = ((uchar *)(img->imageData + 1*img->widthStep))[100];
    low = ((uchar *)(img->imageData + img->height*img->widthStep))[100];

    return(high == low);
}

bool Camera::findStripe()
{
    int i = 150, highest, lowest, mid, highPix, lowPix, midPix;
    QList<CvPoint*> lineList;

    //imgColor = openImage(fileName);
    imgColor = getImageSave(1);
    IplImage *img = cvCreateImage(cvGetSize(imgColor), 8, 1);
    cvCvtColor(imgColor, img, CV_BGR2GRAY);

    while(lineList.length() < 5 && i > 0)
    {
        lineList = getLines(i, img);
        std::cout << lineList.length() << std::endl;
        i -= 25;
    }

    if(lineList.length() > 0)
    {
        lowest = 500;
        highest = 0;

        for(int j = 0; j < lineList.length(); j++)
        {
            if((lineList[j][0].y < lowest) && (lineList[j][0].y > 10))
            {
                lowest = lineList[j][0].y;
            }
            if(lineList[j][0].y > highest)
            {
                highest = lineList[j][0].y;
            }
            //std::cout << "Line " << j << ": " << "(" << lineList[j][0].x << ", " << lineList[j][0].y <<
            //        "), (" << lineList[j][1].x << ", " << lineList[j][1].y << ")" << std::endl;
        }
        std::cout << "High/Low: " << highest << ", " << lowest << std::endl;

        mid = lowest + ((highest - lowest) / 2);
        midPix = ((uchar *)(img->imageData + mid*img->widthStep))[100];
        lowPix = ((uchar *)(img->imageData + (lowest - 10)*img->widthStep))[100];
        highPix = ((uchar *)(img->imageData + (highest + 10)*img->widthStep))[100];

        if(midPix > (lowPix + 50) && midPix > (highPix + 50))
        {
            return true;
        }
        else
        {
            return false;
        }
    }
    else
    {
        return false;
    }
}


QList<CvPoint*> Camera::getLines(int resolution, IplImage *imgIn)
{
    //cvNamedWindow("Orig", CV_WINDOW_AUTOSIZE );
    //cvNamedWindow("Lines", CV_WINDOW_AUTOSIZE );
    IplImage *img = cvCreateImage(cvGetSize(imgColor), 8, 1);
    QList<CvPoint*> lineList;
    cvCopy(imgIn, img, NULL);
    IplImage* edges = cvCreateImage(cvGetSize(img), 8, 1);

    CvMemStorage* storage = cvCreateMemStorage(0);

    CvSeq* lines = 0;

    cvSmooth( img, img, CV_BILATERAL, 5, 5, 10, 10);

    //cvShowImage("Orig",  img);

    cvCanny(img, edges, 20, 60, 3);
    //cvShowImage("Edges", edges);

    lines = cvHoughLines2( edges, storage, CV_HOUGH_STANDARD, 1, CV_PI/180, resolution, 0, 0);

    //std::cout << "150: " << lines->total << std::endl;
    //cvShowImage("Lines150", getLineImage(img, lines));

    for(int i = 0; i < lines->total; i++ )
    {
        float* line = (float*)cvGetSeqElem(lines,i);
        float rho = line[0];
        float theta = line[1];
        double a = cos(theta), b = sin(theta);
        //std::cout << "Theta: " << a << ", " << b << std::endl;
        double x0 = a*rho, y0 = b*rho;
        //std::cout << "Rho: " << x0 << ", " << y0 << std::endl;
        CvPoint point1;
        point1.x = x0;
        point1.y = y0;
        cvLine(img, point1, point1, CV_RGB(255, 255, 255), 3, 8);
        lineList.append((CvPoint*)malloc(sizeof(CvPoint) * 2));
        //        std::cout << "0x: " << 320*(-b) << ", " << x0 + 320*(-b) << std::endl;
        //        std::cout << "0y: " << 240*(a) << ", " << y0 + 240*(a) << std::endl;
        //        std::cout << "1x: " << 320*(-b) << ", " << x0 - 320*(-b) << std::endl;
        //        std::cout << "1y: " << 240*(a) << ", " << y0 - 240*(a) << std::endl;
        lineList[i][0].x = cvRound(x0 + 0*(-b));
        lineList[i][0].y = cvRound(y0 + 0*(a));
        lineList[i][1].x = cvRound(x0 - 320*(-b));
        lineList[i][1].y = cvRound(y0 - 240*(a));
        cvLine( img, lineList[i][0], lineList[i][1], CV_RGB(0,0,0), 1, 8 );
        //std::cout << lineList << std::endl;
    }

    cvSaveImage("2.jpg", img);

    //    CvPoint one, two;
    //    one.x = 0;
    //    one.y = 100;
    //    two.x = 100;
    //    two.y = 200;
    //    cvLine(img, one, two, CV_RGB(255,255,255), 1, 8);
    //    one.x = 300;
    //    one.y = 100;
    //    two.x = 100;
    //    two.y = 200;
    //    cvLine(img, one, two, CV_RGB(255,255,255), 1, 8);
    //    std::cout << img->width << ", " << img->height << std::endl;
    //cvShowImage("Lines", img);

    return lineList;
}
